﻿using System;
using System.Collections.Generic;
using System.Text;
using System.IO;
using System.Threading;
using System.IO.Ports;
using System.Globalization;
using System.Text.RegularExpressions;
using csmatio.types;
using csmatio.io;

namespace GroundStation
{
    class Telemetry
    {
        // time
        UInt32 time;               // [ms]

        // euler angles
        float eul_pitch;           // euler angle, pitch, [deg]
        float eul_roll;            // euler angle, pitch, [deg]
        float eul_heading;         // euler angle, pitch, [deg]

        // 3d position and speed
        float pos_x;               // position, x, [m]
        float pos_y;               // position, y, [m]
        float pos_alt;             // position, altitude, [m]

        // air data computer
        float adc_speed;           // air data computer, indicated speed, [ms^-1]
        float adc_alt;             // air data computer, barometric altitude, [m]
        float adc_temp;            // air data computer, air temperature, [deg C]

        // power management
        float pwr_volt1;           // power management, voltage1, [V]
        float pwr_volt2;           // power management, voltage2, [V]
        float pwr_volt3;           // power management, voltage3, [V]
        float pwr_volt4;           // power management, voltage4, [V]
        float pwr_fuel;            // power management, remainging fuel, [%]

        // engine monitoring system
        float ems_rpm;            // engine monitoring system, shaft rpm, [min^-1]
        float ems_temp_head;       // engine monitoring system, temperature, head temperature, [deg C]
        float ems_temp_exhaust;    // engine monitoring system, temperature, exhaust gases, [deg C]

        SerialPort serialPort = new SerialPort();

        private string _startTime; // formated start time stamp

        string logFileName = "";

        Thread thread;
        Mutex mutex = new Mutex();
        bool keepGoing = false;

        int lastTicks = 0;


        /*private struct ActualData
        {
            float ddd;
            string[] WhoIsWho;

        }*/

        // getters
        public UInt32 Time
        {
            get
            {
                UInt32 ret;

                mutex.WaitOne();
                ret = time;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float EulPitch
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = eul_pitch;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float EulRoll
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = eul_roll;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float EulHeading
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = eul_heading;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PosX
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pos_x;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PosY
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pos_y;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PosAlt
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pos_alt;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcSpeed
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_speed;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcAlt
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_alt;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float AdcTemp
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = adc_temp;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PwrVolt1
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pwr_volt1;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PwrVolt2
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pwr_volt2;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PwrVolt3
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pwr_volt3;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PwrVolt4
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pwr_volt4;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float PwrFuel
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = pwr_fuel;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float EmsRmp
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = ems_rpm;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float EmsTempHead
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = ems_temp_head;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public float EmsTempExhaust
        {
            get
            {
                float ret;

                mutex.WaitOne();
                ret = ems_temp_exhaust;
                mutex.ReleaseMutex();
                return ret;
            }
        }

        public void Init(string port)
        {
            //System.Console.Write("telelmetry: opening port {0}...", port);  
            lastTicks = Environment.TickCount;
            this._startTime = DateTime.Now.ToString("yyyy-MM-dd_hhmmss");
            thread = new Thread(Proc);
            // serial port configuration
            serialPort.PortName = port;
            serialPort.BaudRate = 115200;
            serialPort.WriteTimeout = 1000;
            serialPort.ReadTimeout = 1000;
            try
            {
                serialPort.Open();
            }
            catch (IOException)
            {
                System.Windows.Forms.MessageBox.Show("Cannot open " + port + "!", "Error", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Error);
                System.Console.WriteLine("failed!", port);
                return;
            }
            System.Console.WriteLine("ok", port);

            logFileName = "Tele_" + _startTime + ".log";

            keepGoing = true;
            thread.Start();
        }

        public void Stop()
        {
            if (keepGoing == true)
            {
                keepGoing = false;
                thread.Join(2000);
            }
        }

        private void Proc()
        {
            List<float[]> mdata = new List<float[]>();
            byte[] buf = new byte[1000];
            int tmp;
            int len;
            int index = 0;
            byte sum = 0;

            while (keepGoing)
            {
                try
                {
                    /*string str = serialPort.ReadLine();

                    int cnt = 0;
                    val = "";
                    for (int i = 0; i < str.Length; i++)
                    {
                      if (str[i] == ',')
                      {
                        switch (cnt)
                        {
                          case 0:
                            nav_roll = float.Parse(val, System.Globalization.CultureInfo.InvariantCulture);
                            break;

                          case 1:
                            nav_pitch = float.Parse(val, System.Globalization.CultureInfo.InvariantCulture);
                            break;

                          case 2:
                            nav_heading = float.Parse(val, System.Globalization.CultureInfo.InvariantCulture);
                            break;

                          default:
                            break;
                        }
                        cnt++;
                        val = "";
                        continue;
                      }
                      val += str[i];
                    }
                  }*/

                    tmp = serialPort.ReadByte();
                    if (tmp == 0xAB)
                    {
                        len = serialPort.ReadByte();
                        if (len == 72)
                        {
                            sum = 0;
                            for (int i = 0; i < len; i++)
                            {
                                tmp = serialPort.ReadByte();
                                if (tmp >= 0)
                                {
                                    buf[i] = (byte)tmp;
                                    sum += (byte)tmp;
                                }
                                else
                                {
                                    System.Console.WriteLine("telemetry: wrong data!");
                                    continue;
                                }
                            }
                        }
                        else
                        {
                            System.Console.WriteLine("telemetry: wrong length!");
                        }
                        tmp = serialPort.ReadByte();
                        if (tmp >= 0)
                        {
                            sum -= (byte)tmp;

                            if (sum == 0)
                            {
                                float[] mdatarow = new float[18];
                                mutex.WaitOne();
                                index = 0;

                                time= ConvertUint32Data(ref buf, index);
                                mdatarow[0] = time;
                                index += 4;
                                for (int mi = 1; mi < 18; mi++)
                                {
                                    mdatarow[mi] = ConvertFloatData(ref buf, index);
                                    index += 4;
                                }
                                int mii = 1;
                                //time = mdatarow[mi++];
                                //index += 4;
                                eul_pitch = mdatarow[mii++];
                                //index += 4;
                                eul_roll = mdatarow[mii++];
                                //index += 4;
                                eul_heading = mdatarow[mii++];
                                //index += 4;
                                pos_x = mdatarow[mii++];
                                //index += 4;
                                pos_y = mdatarow[mii++];
                                //index += 4;
                                pos_alt = mdatarow[mii++];
                                //index += 4;
                                adc_speed = mdatarow[mii++];
                                //index += 4;
                                adc_alt = mdatarow[mii++];
                                //index += 4;
                                adc_temp = mdatarow[mii++];
                                //index += 4;
                                pwr_volt1 = mdatarow[mii++];
                                //index += 4;
                                pwr_volt2 = mdatarow[mii++];
                                //index += 4;
                                pwr_volt3 = mdatarow[mii++];
                                //index += 4;
                                pwr_volt4 = mdatarow[mii++];
                                //index += 4;
                                pwr_fuel = mdatarow[mii++];
                                //index += 4;
                                ems_rpm = mdatarow[mii++];
                                //index += 4;
                                ems_temp_head = mdatarow[mii++];
                                //index += 4;
                                ems_temp_exhaust = mdatarow[mii++];


                                using (FileStream fs = new FileStream(logFileName, FileMode.Append, FileAccess.Write, FileShare.ReadWrite))
                                {
                                    using (StreamWriter sw = new StreamWriter(fs))
                                    {
                                        string str = time.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     eul_pitch.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     eul_roll.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     eul_heading.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pos_x.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pos_y.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pos_alt.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     adc_speed.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     adc_alt.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     adc_temp.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pwr_volt1.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pwr_volt2.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pwr_volt3.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pwr_volt4.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     pwr_fuel.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     ems_rpm.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     ems_temp_head.ToString(CultureInfo.InvariantCulture) + "\t" +
                                                     ems_temp_exhaust.ToString(CultureInfo.InvariantCulture);
                                        sw.WriteLine(str);
                                    }
                                }
                                mutex.ReleaseMutex();
                                mdata.Add(mdatarow);
                            }
                        }
                        else
                        {
                            System.Console.WriteLine("telemetry: checksum failed!");
                            continue;
                        }
                    }
                }
                catch (IOException)
                {
                    System.Console.WriteLine("telemetry: io exception!");
                }
                catch (TimeoutException)
                {
                    System.Console.WriteLine("telemetry: com timeout!");
                }
                catch (InvalidOperationException)
                {
                    System.Console.WriteLine("telemetry: invalid operation exception!");
                    keepGoing = false;
                }
                catch (FormatException)
                {
                    System.Console.WriteLine("telemetry: data corrupted!");
                }
                catch (ThreadAbortException)
                {
                    return;
                }
            }
            // dddd
            serialPort.Close();
            if (mdata.Count > 0)
            {
                try
                {
                    float[][] mlData = mdata.ToArray(); // list to array conversion - possible improvement in future
                    MLSingle mlSingle = new MLSingle("data", mlData); // creates matlab array of single values as "data" variable
                    List<MLArray> list = new List<MLArray>(); // list of matlab array
                    list.Add(mlSingle); // adding data to matlab list
                    new MatFileWriter("Tele_" + this._startTime + ".mat", list, false); // creates .mat file: string filename, array data, bool compression
                }
                catch (System.OutOfMemoryException e)
                {
                   // TODO
                }
            }
        }

        private UInt16 ConvertUint16Data(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            UInt16 ret = 0;

            try
            {
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];

                ret = BitConverter.ToUInt16(val, 0);
            }
            catch (Exception)
            { }

            return ret;
        }

        private UInt32 ConvertUint32Data(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            UInt32 ret = 0;

            try
            {
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];

                ret = BitConverter.ToUInt32(val, 0);
            }
            catch (Exception)
            { }

            return ret;
        }

        private float ConvertFloatData(ref byte[] buf, int index)
        {
            byte[] val = new byte[4];
            float ret = 0;

            try
            {
                val[0] = buf[index + 0];
                val[1] = buf[index + 1];
                val[2] = buf[index + 2];
                val[3] = buf[index + 3];

                ret = BitConverter.ToSingle(val, 0);
            }
            catch (Exception)
            { }

            return ret;
        }
    }
}
